#coding=utf-8
import time
import threading
from shapely.geometry import Point, Polygon
import app
from controller import geometric,physical
from controller.params import Params
from definations.definations import *
from basic_math import geolib
import numpy as np
'''
    脚部坐标到关节的映射
'''



class Kinematic_Controller:
    '''
    运动学控制器
    '''

    def __init__(self,rootObject):
        '''
        初始化控制器
        :param base:
        :param lFoot:
        :param rFoot:
        '''
        self.rootObject=rootObject
        self.physical=rootObject.physical
        self.terminals=[self.rootObject.lFoot,self.rootObject.rFoot]

        #   Terminal 和 Chain
        self.base=self.rootObject.base
        self.lFoot=self.rootObject.lFoot
        self.rFoot=self.rootObject.rFoot
        self.lChain=self.rootObject.lChain
        self.rChain=self.rootObject.rChain

        #   支撑状态
        self.modelBalanceState=STATE.STATE_UNSTABLE
        self.physicalBalanceState=STATE.STATE_UNSTABLE
        self.tsupFoot=[self.rootObject.lFoot,self.rootObject.rFoot]
        self.tfreeFoot=None
        self.tsupChain=[self.lChain,self.rChain]
        self.tfreeChain=None
        self.massCenter=geolib.GeoEntity()

        #   运动状态
        self.approachingTarget=False
        self.keepBalance=True
        self.timeRemain=0.0

        #   平衡器
        self.modelFlag=True
        self.physicalFlag=False
        threading.Thread(target=self.CtrlCycle).start()
    def SetRefCoord(self,target:geometric.Terminal):
        '''
        基于机器人支撑，构建坐标系
        :return:
        '''
        ref=geometric.Terminal()
        ref.Mimic(target)
        for t in self.terminals:
            t.SetRefCoord(ref)
        self.base.SetRefCoord(ref)
    def UpdateTragetSupporter(self):
        '''
        更新目标支撑
        :return:
        '''
        if self.tsupFoot==self.lFoot:
            self.tsupChain=self.lChain
            self.tfreeChain=self.rChain
            self.tfreeFoot=self.rFoot
        elif self.tsupFoot==self.rFoot:
            self.tsupChain = self.rChain
            self.tfreeChain = self.lChain
            self.tfreeFoot = self.lFoot
        elif self.tsupFoot==None:
            self.tsupChain=None
            self.tfreeChain=[self.lChain,self.rChain]
            self.tfreeFoot=[self.lFoot,self.rFoot]
        else:
            self.tsupFoot = [self.rootObject.lFoot, self.rootObject.rFoot]
            self.tfreeFoot = None
            self.tsupChain = [self.lChain, self.rChain]
            self.tfreeChain = None

    def ResetTarget(self):
        for t in self.terminals:
            t.ResetTarget()
        self.base.ResetTarget()

    def StartApproach(self,bx=0,by=0,bz=0,br=0,bp=0,byaw=0,\
                      rx=0,ry=0,rz=0,rr=0,rp=0,ryaw=0,\
                      lx=0,ly=0,lz=0,lr=0,lp=0,lyaw=0,t=0.5):
        self.ResetTarget()
        self.base.target.Translate(bx,by,bz)
        self.base.target.Rotate(br,bp,byaw)
        self.rFoot.target.Translate(rx,ry,rz)
        self.rFoot.target.Rotate(rr,rp,ryaw)
        self.lFoot.target.Translate(lx,ly,lz)
        self.lFoot.target.Rotate(lr,lp,lyaw)


    def InitApproach(self,t=1.0):
        self.ResetTarget()
        self.timeRemain = t
        self.approachingTarget = True

    def UpdateCoordinate(self):
        '''
        基于支撑状态更新整体坐标系
        :return:
        '''
        if self.physical.supportFoot != None:
            self.SetRefCoord(self.physical.supportFoot)


    def ResolveChain(self):
        '''
            基于Terminal位置逆解机器人的关节
            :return:
        '''
        if type(self.tsupChain)==geometric.FootTerminal:
            self.tsupChain.ResolveChain(autoAdjust='base')
            self.tfreeChain.ResolveChain(autoAdjust='terminal')
        else:
            self.lChain.ResolveChain(autoAdjust='terminal')
            self.rChain.ResolveChain(autoAdjust='terminal')

    def BuildChain(self):
        '''
        基于关节位置正解机器人
        :return:
        '''
        if self.physical.supportChain==None:
            self.lChain.BuildChain(autoAdjust='terminal')
            self.rChain.BuildChain(autoAdjust='terminal')
        self.physical.supportChain.BuildChain(autoAdjust='base')
        self.physical.freeChain.BuildChain(autoAdjust='terminal')

    def UpdateMassCenter(self):
        '''
        更新重心
        :return:
        '''
        c=np.zeros(3)
        mass=0.0
        c+=self.lChain.massCenter*self.lChain.mass
        mass+=self.lChain.mass
        c+=self.rChain.massCenter*self.rChain.mass
        mass += self.rChain.mass
        c+=(self.base.VectorPose()+self.base.gcBias)*self.base.weight
        mass += self.base.weight
        c/=mass
        self.massCenter=geolib.GeoEntity(c[0],c[1],c[2])

    def BalanceByModel(self,ratio=0.1,zTrans=0.005):
        '''
        机器人保持平衡
        :return: 是否稳定
        '''

        if type(self.tsupFoot)==list:
            self.modelBalanceState=STATE.STATE_UNSTABLE
        elif type(self.tsupFoot)==geometric.FootTerminal:
            x, y, z = self.tsupFoot.DistancebyAxis(self.physical.massCenter)
            projectPoint=Point(x,y)
            polygen=Polygon(self.rootObject.config['foot_stable_polygen'])
            if polygen.contains(projectPoint):
                self.modelBalanceState=STATE.STATE_STABLE
            else:
                if self.keepBalance:
                    self.base.Translate(x*ratio,y*ratio,zTrans)
                self.modelBalanceState=STATE.STATE_UNSTABLE

    def BalanceByPhysical(self,ratio=0.1):
        '''
        使用脚底实现机器人保持平衡
        :return: 是否稳定
        '''
        if type(self.tsupFoot)!=geometric.FootTerminal:
            return
        else:
            x, y = self.tsupFoot.pressCenter[0], self.tsupFoot.pressCenter[1]
            projectPoint = Point(x, y)
            polygen = Polygon(self.rootObject.config['foot_stable_polygen'])
            if polygen.contains(projectPoint):
                self.physicalBalanceState = STATE.STATE_STABLE
            else:
                #self.base.Translate(0,-y*ratio,0.001)
                self.tsupChain.joints[0]-=y*ratio
                self.tsupChain.BuildChain(autoAdjust='base')
                self.tfreeChain.ResolveChain(autoAdjust='terminal')
                self.physicalBalanceState = STATE.STATE_UNSTABLE


    def Approach(self):
        '''
        向目标点运动
        :return:
        '''
        steps = int(self.timeRemain * Params.controlParams['freq_med'])
        self.timeRemain-=1.0/Params.controlParams['freq_med']
        if steps<1:
            self.approachingTarget=False
            return
        self.lFoot.Approach(steps)
        self.rFoot.Approach(steps)
        self.base.Approach(steps)
        self.lFoot.ApproachRot(steps)
        self.rFoot.ApproachRot(steps)
        self.base.ApproachRot(steps)


    def CtrlCycle(self):
        '''
        控制循环程序
        :return:
        '''
        while True:
            time.sleep(0.05)
            t=time.time()
            self.physical.UpdateSupportState()  # 更新物理学模型
            self.UpdateTragetSupporter()
            self.UpdateCoordinate()
            self.ResolveChain()
            self.physical.Update()
            self.UpdateMassCenter()     #   更新重心位置

            '''
                确定平衡装填
            '''
            if self.modelFlag:
                self.BalanceByModel()
            # if self.physicalFlag:
            #     self.BalanceByPhysical()
            if self.approachingTarget:
                self.Approach()  #
            if self.modelBalanceState==STATE.STATE_FALLING:
                '''
                非平衡状态，落脚
                '''
                pass
            if self.rootObject.panel.ui_Main.autoCheck.isChecked():
                self.physical.Sync()

    def StartPace(self):
        '''
        开始走路
        :return:
        '''
        threading.Thread(target=self.Pace).start()
    def Pace(self):
        '''
        前进！
        :return:
        '''
        slp=1.0/Params.controlParams['freq_med']
        for foot in [self.lFoot,self.rFoot,self.lFoot,self.rFoot,self.lFoot,self.rFoot]:
            self.tsupFoot=foot
            self.modelFlag = True
            time.sleep(slp*2)
            #   等待模型稳定
            self.WaitModelStable()
            #   缓慢转动身体，直到自由脚感受不到压力
            self.InitApproach(t=1)
            if self.tsupFoot==self.lFoot:
                self.base.target.Translate(0,0.01,0.01)
                self.base.target.roll=-0.1
            elif self.tsupFoot==self.rFoot:
                self.base.target.Translate(0,-0.0,0.01)
                self.base.target.roll=0.1
            self.WaitApproaching()
            #   抬起非支撑脚
            self.InitApproach(t=2)
            self.tfreeFoot.target.Translate(0,0,0.02)
            self.WaitApproaching()
            #   落下非支撑脚
            self.modelFlag=False
            self.InitApproach(t=2)
            self.base.target.roll = 0.0
            self.base.target.y=0
            self.tfreeFoot.target.Translate(0, 0, -0.03)
            self.WaitApproaching(touchStop=True,target=self.tfreeFoot)

        self.tsupFoot=[self.lFoot,self.rFoot]


    def WaitApproaching(self,touchStop=False,releaseStop=False,target=None,relay=0.01):
        while self.approachingTarget:
            time.sleep(relay)
            if touchStop:
                if target.isSupport:
                    self.approachingTarget=False
            if releaseStop:
                if not target.isSupport:
                    self.approachingTarget = False
    def WaitModelStable(self,accu=0.01):
        while self.modelBalanceState!=STATE.STATE_STABLE:
            time.sleep(accu)

    def WaitPhysicalBalance(self,accu=0.01):
        while self.physicalBalanceState!=STATE.STATE_STABLE:
            print(self.physicalBalanceState)
            time.sleep(accu)